from os import sep
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import math

data_names = ["ts", "px", "py", "pz", "roll", "pitch", "yaw", "vn", "ve", "vd",
              "gxb", "gyb", "gzb", "axb", "ayb", "azb", "gps_px", "gps_py", "gps_pz"]
# ref: https://pandas.pydata.org/docs/reference/api/pandas.read_csv.html
sim_data = pd.read_csv("/tmp/imu_integ_fusion.txt",
                       sep="\s+",
                       header=None,
                       names=data_names)

# position
plt.figure("Trajectory")
plt.plot(sim_data["px"], sim_data["py"], '.')

plt.figure("Position")
plt.subplot(3, 1, 1)
plt.plot(sim_data["ts"], sim_data["px"], '.')
plt.plot(sim_data["ts"], sim_data["gps_px"], '.')
plt.subplot(3, 1, 2)
plt.plot(sim_data["ts"], sim_data["py"], '.')
plt.plot(sim_data["ts"], sim_data["gps_py"], '.')
plt.subplot(3, 1, 3)
plt.plot(sim_data["ts"], sim_data["pz"], '.')
plt.plot(sim_data["ts"], sim_data["gps_pz"], '.')

plt.figure("Attitude")
plt.subplot(3, 1, 1)
plt.plot(sim_data["ts"], sim_data["roll"], '.')
plt.subplot(3, 1, 2)
plt.plot(sim_data["ts"], sim_data["pitch"], '.')
plt.subplot(3, 1, 3)
plt.plot(sim_data["ts"], sim_data["yaw"], '.')

plt.figure("Velocity")
plt.subplot(3, 1, 1)
plt.plot(sim_data["ts"], sim_data["vn"], '.')
plt.subplot(3, 1, 2)
plt.plot(sim_data["ts"], sim_data["ve"], '.')
plt.subplot(3, 1, 3)
plt.plot(sim_data["ts"], sim_data["vd"], '.')

plt.figure("IMU bias")
plt.subplot(2, 1, 1)
plt.plot(sim_data["ts"], sim_data["gxb"], '.')
plt.plot(sim_data["ts"], sim_data["gyb"], '.')
plt.plot(sim_data["ts"], sim_data["gzb"], '.')

plt.subplot(2, 1, 2)
plt.plot(sim_data["ts"], sim_data["axb"], '.')
plt.plot(sim_data["ts"], sim_data["ayb"], '.')
plt.plot(sim_data["ts"], sim_data["azb"], '.')

plt.show()
